/*
 * JRoverSub.cpp
 *
 *  Created on: Dec 2, 2012
 *      Author: bouchier
 */
/*! \file JRoverSub.cpp
 * \brief The source file for the jRoverSubsumption class. Also contains main().
 *
 *
 * <H1>
 * Build Configuration
 * </H1>
 *
 * This project depends on the following peer projects being at the same directory level:
 * - CQEI2C
 * - Metro
 * - CQEIMEncoder
 * - qetime
 * - PID
 * - RCTest
 * - ControlledMotor
 * - JRoverMotors
 * - JRoverSensors
 * - ROSIF
 * - Runtimer
 *
 * Edit the project properties as follows to reference them as includes, link objects, and referenced projects.
 *
 * Under C/C++ Build -> Settings, Tool Settings tab, TerkOS C++ Compiler group, Directories settings
 * add the following paths
 * to the terkos paths that are already there. This adds them to the include path for compilation
 * - ../../CQEI2C
 * - ../../Metro
 * - ../../qetime
 * - ../../ControlledMotor
 * - ../../JRoverMotors
 * - ../../JRoverSensors
 * - ../../ROSIF
 * - ../../Runtimer
 *
 * Under C/C++ Build -> Settings, Tool Settings tab, TerkOS C++ Linker group, Miscellaneous settings, add
 * the following "other objects". This tells the linker to link to qetime.o & CQEI2C.o.
 * - ../../CQEI2C/Debug/CQEI2C.o
 * - ../../Metro/Debug/Metro.o
 * - ../../CQEIMEncoder/Debug/CQEIMEncoder.o
 * - ../../qetime/Debug/qetime.o
 * - ../../PID/Debug/pid.o
 * - ../../RCTest/Debug/RCTest.o
 * - ../../ControlledMotor/Debug/ControlledMotor.o
 * - ../../JRoverMotors/Debug/JRoverMotors.o
 * - ../../JRoverSensors/Debug/JRoverSensors.o
 * - ../../ROSIF/Debug/ROSIF.o
 * - ../../usonicRanger/Debug/usonicRanger.o
 * - ../../Runtimer/Debug/Runtimer.o
 *
 * In the Project References group, check the following projects. This builds them before the current project.
 * CQEI2C
 * Metro
 * CQEIMEncoder
 * qetime
 * PID
 * ControlledMotor
 * RCTest
 * JRoverMotors
 * ROSIF
 * usonicRanger
 *
 */
#define DEFAULT_TEST 2
#define DEFAULT_SPEED 10.0

// default command lines to run. Add new command lines here & comment out all but one
char *default_argv[] = {(char*)"JRoverSub"};
int default_argc = 1;

#include <stdio.h>
#include <getopt.h>
#include <stdlib.h>   /* calloc, strtol, getenv */
#include <string.h>   /* String function definitions */
#include <unistd.h>
#include <keypad.h>
#include <textlcd.h>
#include <qegpioint.h>
#include <qeanalog.h>
#include "Metro.h"
#include "CQEI2C.h"
#include "qetime.h"
#include "ROSIF.h"
#include "JRoverMotors.h"
#include "JRoverSensors.h"
#include "Runtimer.h"
#include "Layer.h"
#include "Stop.h"
#include "Cruise.h"
#include "RangeBackoff.h"
#include "RangeFollower.h"
#include "JRoverSub.h"

using namespace std;

CQEI2C i2c = CQEI2C();		// instantiate the I2C driver
CQEGpioInt &gpio = CQEGpioInt::GetRef();
CQEAnalog &analog = CQEAnalog::GetRef();
JRoverMotors jRoverMotors = JRoverMotors();
JRoverSensors jRoverSensors = JRoverSensors();
ROSIF rosif = ROSIF();
Runtimer loopTimer = Runtimer((char*)"loopTimer");
JRoverSub jRoverSub = JRoverSub();
CKeypad &keypad = CKeypad::GetRef();
CTextLcd &lcd = CTextLcd::GetRef();

JRoverSub::JRoverSub() {

}

JRoverSub::~JRoverSub() {}

//! Configure the layers based on the requested algorithm
void JRoverSub::setAlgorithm(int algIn)
{
	algorithm = algIn;
	printf("Configuring subsumption with algorithm %d\n", algorithm);

	// instantiate all the layers
	stopBot = new StopBot();
	cruise = new Cruise(DEFAULT_SPEED);
	rangeBackoff = new RangeBackoff();
	rangeFollower = new RangeFollower();

	// initialize subsumption algorithm
	switch (algorithm) {
	case 1:
		// algorithm 1: basic cruise for a few seconds
		algLayers[0] = cruise;
		cruise->setRunTime(3);
		algLayers[1] = stopBot;
		layerList = &algLayers[0];
		layerCount = 2;
		runTime = 5;
		break;
	case 2:
		// algorithm 2: follow with ultrasonic sensors
		algLayers[0] = rangeBackoff;
		algLayers[1] = rangeFollower;
		algLayers[2] = stopBot;
		layerList = &algLayers[0];
		layerCount = 3;		// default, stopped
		runTime = 0;
		break;

	default:
		// invalid algorithm
		fprintf(stderr, "invalid algorithm %d, exiting\n", algorithm);
		exit(-1);
	}

	activeLayer = layerList[0];	// start with first layer in current job
	arbitrateEnable = 1;
	halt = 0;

}

//! Start running the subsumption algorithm to drive the robot
/*
 * Assumes ROSIF, JRoverMotors and JRoverSensors have already been initialized
 * and any health checks performed
 */
int JRoverSub::go()
{
	int loopCnt = 0;
	int rv;
	CQETime::tick_t startTime = CQETime::ticks();
	//missionControl->setSubPtr(sub_p);	// set pointer to Subsumption object in missionControl

	lcd.Clear();
	lcd.printf("Press X to exit");

	Metro heartMetro = Metro(50);		// metronome ticks every 50ms

	// main run loop for exploring
	while (!keypad.KeyCancel()) {
		if (heartMetro.check()) {		// metronome has ticked, time to run the algorithm
			// read sensor data
			jRoverSensors.pollSensors();	// update sensor readings from jRover

			// evaluate each layer & choose the active one & pass its output to the motors
			rv = arbitrate();		// do the subsumption layer evaluation & arbitration, which drives motor
			if (rv < 0)				// quit if no layers want to control robot
				break;				// quit running subsumption & go do something more interesting

			// publish motor telemetry every 50ms if ROS is enabled
			if (rosif.rosEnabled) {
				rosif.publishRobotTelemetry(); // RobotTelemetry takes avg 9 ms to publish
			}
		}
		if (rosif.rosEnabled) {
			rosif.poll();
		}

		loopCnt++;
		if (runTime && CQETime::timeout(runTime, startTime)) {
			printf("Terminating subsumption because of end of runtime\n");
			break;
		}
	}

	// finished this run, return & maybe get asked to do another run
	return 0;
}

//! Choose the highest priority layer & send its requested speeds to jRoverMotors
/*!
 * Must be called every frame, to keep the PID updated
 *
 * \return 0 for normal return, -1 if no layers want to run. This is the way to stop processing.
 */
int JRoverSub::arbitrate()
{
    int i = 0;
    int rv = 0;				// default return value, continue running

    if (arbitrateEnable) {
        // step through Layer objects in priority order until we find the first one that wants to take control
        for (i = 0; i < layerCount; i++) { // step through tasks
        	layerList[i]->eval();				// evaluate whether this layer wants to run
            if (layerList[i]->getFlag()) break;       // if yes, subsume lower priority layers by breaking to run its motor request
        }

        // ask the highest winning layer for its speed request
        if ((i == layerCount) && (!(layerList[i-1]->getFlag()))) {	// iterated through last layer & its flag is clear
        	printf("arbitrate: no layers want to run\n");
        	rqVLinear = rqVAngular = 0.0;					// no layers want to run
        	rv = -1;
        } else {
        	activeLayer = layerList[i];        // highest priority layer is winner
        	activeLayerName = activeLayer->getLayerName();
            activeLayer->getSpeedRqst(rqVLinear, rqVAngular);	// get speed rqst from active layer
            if (activeLayer != lastActiveLayer) {
            	lastActiveLayer = activeLayer;
                printf("arbitrate: active layer: %s\n", activeLayerName);
           }
        }

        // send command to motors
        jRoverMotors.driveRover(rqVLinear, rqVAngular);
		jRoverMotors.updateAllMotors();			// run the PID loop
    }
    return rv;
}


void JRoverSub1(int i)
{
	printf("test 1, algorithm %d, ROS %s active\n", i, rosif.rosEnabled?"IS":"IS NOT");

	jRoverSub.setAlgorithm(i);
	jRoverSub.go();
}
void usage()
{
    printf("Usage: jRover [OPTIONS]\n"
    "\n"
    "Options:\n"
    "  -h, --help                   Print this help message\n"
    "  -t<testnum>					Run the specified test\n"
    "Tests:\n"
    "1: Drive jRover from R/C or ROS with subsumption algorithm\n"
    "\n"
    );
}


int main(int argc, char *argv[])
{
	int i;
	int testNum = DEFAULT_TEST;			// which test to execute
	int rosFlag;

	// if no arguments (e.g. run from TerkIDE) use default arguments from list above
    if (argc == 1) {
    	printf("No args provided; using default: ");
    	for(i=0; i<default_argc; i++) {
    		printf("%s ", default_argv[i]);
    	}
    	printf("\n");
    	argc = default_argc;
    	argv = default_argv;
    }

    // parse options & arguments
    int option_index = 0, opt;
    static struct option loptions[] = {
        {"help",       no_argument,       0, 'h'},
        {0,0,0,0}
    };


    while(1) {
        opt = getopt_long (argc, argv, "ht:",
                           loptions, &option_index);
        if (opt==-1) break;

        switch (opt) {
        case 'h': usage(); break;
        case 't': testNum = strtol(optarg, NULL, 10); break;
        default: usage(); exit(0);
        }
    }

    // Initialize ROS & jRover
	printf("Initializing ROS, motors, sensors\n");
	rosFlag = rosif.initRos();

	jRoverMotors.initMotors();		// always start with this
	jRoverSensors.initSensors();	// and this

    switch (testNum) {
    case 1: JRoverSub1(1); break;
    case 2: JRoverSub1(2); break;
    default: printf("Invalid test number\n"); exit(0);
    }
}
